#include "zeus_s3_odom/zeus_s3_odom.hpp"

DriverOdomPub::DriverOdomPub():
        start_flag_(0),
        last_twist_time_(0),
        rev_left_(0),
        rev_left_1_(0),
        rev_right_(0),
        rev_right_1_(0),
        last_time_(0){
        }

DriverOdomPub::~DriverOdomPub()
{
    boost::mutex::scoped_lock look(mutex_);

    printf("DirvePub deinit.\n");

}

//接收驱动器速度
void DriverOdomPub::motor_feedback_callback( const zeus_s3_msgs::Feedback::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_speed_ = *msg.get();
        rev_speed_flag = true;
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }


}

void DriverOdomPub::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_flag = true;
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

void DriverOdomPub::ResetOdometry() {
  position_x_ = 0.0;
  position_y_ = 0.0;
  theta_ = 0.0;
}

void DriverOdomPub::ResetOdomIntegratorCallback(
    const std_msgs::Bool::ConstPtr &msg) {
  if (msg->data) ResetOdometry();
}

void DetachRobot(int signal) {
//   robot.Disconnect();
//   robot.Terminate();
}

void DriverOdomPub::PublishOdometryToROS(double linear, double angular,
                                              double dt) {
  // perform numerical integration to get an estimation of pose
  linear_speed_ = linear;
  steering_angle_ = angular;

  // propagate model model
  asc::state_t state =
      model_.Propagate({position_x_, position_y_, theta_},
                       {linear_speed_, steering_angle_}, 0, dt, dt / 100);
  position_x_ = state[0];
  position_y_ = state[1];
  theta_ = state[2];

  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);

  // publish tf transformation
  if (publish_odom_transform_) {
    transformStamped_.header.stamp = now_;
    transformStamped_.header.frame_id = odom_frame_;
    transformStamped_.child_frame_id = base_frame_;

    transformStamped_.transform.translation.x = position_x_;
    transformStamped_.transform.translation.y = position_y_;
    transformStamped_.transform.translation.z = 0.0;
    transformStamped_.transform.rotation = odom_quat;

    br_.sendTransform(transformStamped_);
  }

  // publish odometry and tf messages
  odom_.header.stamp = now_;
  odom_.header.frame_id = odom_frame_;
  odom_.child_frame_id = base_frame_;

  odom_.pose.pose.position.x = position_x_;
  odom_.pose.pose.position.y = position_y_;
  odom_.pose.pose.position.z = 0.0;
  odom_.pose.pose.orientation = odom_quat;

  odom_.twist.twist.linear.x = linear_speed_;
  odom_.twist.twist.linear.y = 0.0;
  odom_.twist.twist.angular.z = linear_speed_ / wheelbase_ * std::tan(steering_angle_);

  odom_.pose.covariance = {
    0.001,      0.0,        0.0,        0.0,        0.0,        0.0,
    0.0,        0.001,      0.0,        0.0,        0.0,        0.0,
    0.0,        0.0,        1000000.0,  0.0,        0.0,        0.0,
    0.0,        0.0,        0.0,        1000000.0,  0.0,        0.0,
    0.0,        0.0,        0.0,        0.0,        1000000.0,  0.0,
    0.0,        0.0,        0.0,        0.0,        0.0,        1000.0};
  odom_.twist.covariance = {
    0.001,      0.0,        0.0,        0.0,        0.0,        0.0,
    0.0,        0.001,      0.0,        0.0,        0.0,        0.0,
    0.0,        0.0,        1000000.0,  0.0,        0.0,        0.0,
    0.0,        0.0,        0.0,        1000000.0,  0.0,        0.0,
    0.0,        0.0,        0.0,        0.0,        1000000.0,  0.0,
    0.0,        0.0,        0.0,        0.0,        0.0,        1000.0};

//   std::cerr << "linear: " << linear_speed_ << " , angular: " << steering_angle_
//             << " , pose: (" << position_x_ << "," << position_y_ << ","
//             << theta_ << ")" << std::endl;
  odom_pub_.publish(odom_);
}

void DriverOdomPub::handle_speed_data(void)
{
    double steering_angle = 0;
    now_ = ros::Time::now();
    delta_time_ = (now_ - last_time_).toSec();
    if(start_flag_)
    {
        accumulation_y_ = 0.0;
        accumulation_x_ = 0.0;
        accumulation_th_ = 0.0;

		accumulation_left_ = 0.0;
    	accumulation_right_ = 0.0;
		accumulation_right_1_ = 0.0;
		accumulation_left_1_ = 0.0;

		delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;
        delta_time_ = 0;
        last_time_ = now_;
        start_flag_ = false;
        ROS_INFO("Drive topic and Odom successfully connected  !");
        // ROS_INFO_STREAM("The Code Version is" << code_version_);

    }

    steering_angle = current_speed_.front_left_joint.position;                   //用于前轮转向角
    delta_left_1_ = current_speed_.rear_left_encoder.velocity * delta_time_;     //作为阿克曼后左轮
    delta_right_1_ = -(current_speed_.rear_right_encoder.velocity * delta_time_);   //作为阿克曼后右轮

	accumulation_right_1_ += (delta_right_1_/(wheel_diameter_4wd_/2.0));            //弧度，阿克曼的后右轮转角
	accumulation_left_1_ += (delta_left_1_/(wheel_diameter_4wd_/2.0));              //弧度，阿克曼的后左轮转角

	left_right_wheel_state_.front_left_joint.position  = 0;
	left_right_wheel_state_.rear_right_joint.position = accumulation_right_1_;
	left_right_wheel_state_.rear_left_joint.position = accumulation_left_1_;

	wheel_state_join_.publish(left_right_wheel_state_);                 //  发布轮子的转动角度

    double left_vel = current_speed_.rear_left_encoder.velocity;
    double right_vel = -current_speed_.rear_right_encoder.velocity;

    double linear_velocity = (left_vel + right_vel) / 2.0;
    // printf("linear_velocity:%f \r\n",linear_velocity);
    // printf("steering_angle:%f \r\n",steering_angle);
    PublishOdometryToROS(linear_velocity, steering_angle, delta_time_);
    
    last_time_ = now_;

}

//主循环
void DriverOdomPub::loop() 
{
    ros::NodeHandle n("~");
    ros::Rate loop_rate(100);
    //获取配置参数
    n.param<std::string>("odom_frame", odom_frame_,std::string("odom"));
    n.param<std::string>("base_frame", base_frame_,std::string("base_footprint"));
    n.param<bool>("publish_odom_transform",publish_odom_transform_,false);
    n.param<double>("wheel_diameter_4wd",wheel_diameter_4wd_,0.216);        //直径
    n.param<double>("wheel_track_4wd",wheel_track_4wd_,0.56);           // in meter (left & right wheel distance)
    n.param<double>("wheelbase",wheelbase_,0.6);                        // in meter (front & rear wheel distance)
    n.param<double>("wheel_radius",wheel_radius_,0.108);                 // in meter
    n.param<double>("gear_reduction_4wd",gear_reduction_4wd_,25);        // 1:25  ,turn to: 1:15
    n.param<double>("max_steer_angle",max_steer_angle_,0.39);           // in rad,  for inner wheel
    n.param<double>("max_steer_angle_central",max_steer_angle_central_,0.33);          // max central angle
    n.param<double>("max_linear_speed",max_linear_speed_,1.36);          // // in m/s

    odom_pub_ = n.advertise<nav_msgs::Odometry>("/odom", 10);
    speed_pid_ = n.advertise<geometry_msgs::Twist>("/robot_cmd_vel",10);
    cmd_sub_ = n.subscribe<geometry_msgs::Twist>("/zeus_speed",100,&DriverOdomPub::cmd_vel_callback,this);      //接受上层下发的速度指令
    motor_driver_sub_ = n.subscribe<zeus_s3_msgs::Feedback>("/motor_driverFeedback",100,&DriverOdomPub::motor_feedback_callback,this);      //接受上层下发的速度指令
    wheel_state_join_=n.advertise<zeus_s3_msgs::Feedback>("/motor_join_potion",10);
    integrator_reset_subscriber_ = n.subscribe<std_msgs::Bool>("/reset_odom_integrator", 5,&DriverOdomPub::ResetOdomIntegratorCallback, this);

    now_ = ros::Time::now();

    while(ros::ok())
    {    
        if(cmd_vel_flag)
        {
            //发布zeus_speed话题速度  
            speed_pid_.publish(current_twist_);
            //清标志
            cmd_vel_flag = false;
        }
            
        if(rev_speed_flag)
        {
            //计算里程
            //里程发布
            handle_speed_data();
            //清标志
            rev_speed_flag = false;
        }
       
            ros::spinOnce();
            loop_rate.sleep();
            

    }


}
int main(int argc, char **argv) {
  // setup ROS node
  ros::init(argc, argv, "zeus_s3_odom_node");
  DriverOdomPub drivr_s3_odom;
  std::signal(SIGINT, DetachRobot);
  drivr_s3_odom.loop();
  return 0;

}
